/*
  This file lets you write waypoints to the EEPROM of the Arduino Pro Mini
  Specifically designed for use with the ArduDriver series
  Coded by Ritchie Wilson
*/

#include "WConstants.h"
#include <WProgram.h>
#include <inttypes.h>
#include <math.h>
#include <avr/eeprom.h>

// INCLUDE YOUR MISSION FILE - CHANGE THE NAME TO YOUR FILE
#include "mission_example.h"

#define EEPROM_MAX_ADDR = 36;
#define WP_RADIUS = 5;

struct Location {
  uint8_t  id;
  uint8_t  stg;
  int32_t  lat;
  int32_t  lon;
  int32_t  alt;
};
struct Location WP		= {0,0,0,0,0};

const float t7			= 10000000.0;
uint8_t waypoint_total;
boolean success = false;
byte j;
pinMode(13, OUTPUT);

// Home location
// 0-3 lat, 4-7 lon, 8-11 alt, 12 waypoint total, 13 radius of wp target
void setup()
{
	waypoint_total = (sizeof(mission) / 14);
	eeprom_write_byte((uint8_t*)12,waypoint_total);
	eeprom_write_byte((uint8_t*)13,WP_RADIUS);
	read_mission();
}

void loop()
{
  for (byte i = 0; i < 30; i++){
    delay(1000);
    read_switch();
    if (mode == 1){
      digitalWrite(13, HIGH);
    } else { digitalWrite(13, LOW); }
  }
  switch(mode){
    case 0:
      read_mission();
      break;
    case 1:
      write_mission();
      break;
  }
}

void write_mission()
{
	struct Location loc;
	int count = 0;
	// Put mission array into Loccation structure
	for (byte i = 0; i < waypoint_total; i++){
		loc.id 		= (uint8_t) mission[i][0];
		loc.stg 	= (uint8_t) mission[i][1];
		loc.lat 	= (long)(mission[i][2] * t7);
		loc.lon 	= (long)(mission[i][3] * t7);
		loc.alt 	= (long)(mission[i][4]);
		// Write Location into EEPROM
		j = (i + 1);
		if (j < 36){
		write_WP(j, loc);
		count++;
		Serial.println(count & " waypoints parsed");
		}
	}
}

void write_WP(uint8_t addr, struct Location loc1)  // just until waypointer writer is done
{
  int addr;
  int addr2;
  if (addr == 0){
    eeprom_write_dword((uint32_t*)addr, loc1.lat);
    addr += 4;
    eeprom_write_dword((uint32_t*)addr, loc1.lon);
    addr += 4;
    eeprom_write_dword((uint32_t*)addr, loc1.alt);
    addr++;
    eeprom_write_byte((uint8_t*)addr, loc1.id);
    addr++;
    eeprom_write_byte((uint8_t*)addr, loc1.stg);
  } else {
    if (addr <= EEPROM_MAX_ADDR){
      addr2 = addr * 14;
      eeprom_write_byte((uint8_t*)addr2, loc1.id);
      addr2++;
      eeprom_write_byte((uint8_t*)addr2, loc1.stg);
      addr2++;
      eeprom_write_dword((uint32_t*)addr2, loc1.lat);
      addr2 += 4;
      eeprom_write_dword((uint32_t*)addr2, loc1.lon);
      addr2 += 4;
      eeprom_write_dword((uint32_t*)addr2, loc1.alt);
    } else {
      Serial.println("Unable to add waypoint to " & addr);
    }
  }
}



void load_WP(uint8_t addr)  // load from EEPROM
{
  int addr;
  int addr2;
  if (addr == 0){
    WP.lat = eeprom_read_dword((uint32_t*)addr);
    addr += 4;
    WP.lon = eeprom_read_dword((uint32_t*)addr);
    addr += 4;
    WP.alt = eeprom_read_dword((uint32_t*)addr);
    addr++;
    WP.id = eeprom_read_byte((uint8_t*)addr);
    addr++;
    WP.stg = eeprom_read_byte((uint8_t*)addr);
  } else {
    if (addr <= EEPROM_MAX_ADDR){
      addr2 = addr * 14;
      WP.id = eeprom_read_byte((uint8_t*)addr2);
      addr2++;
      WP.stg = eeprom_read_byte((uint8_t*)addr2);
      addr2++;
      WP.lat = eeprom_read_dword((uint32_t*)addr2);
      addr2 += 4;
      WP.lon = eeprom_read_dword((uint32_t*)addr2);
      addr2 += 4;
      WP.alt = eeprom_read_dword((uint32_t*)addr2);
    } else {
      Serial.println("Unable to load waypoint from " & addr);
    }
  }
  Serial.print(WP.lat);
  Serial.print(WP.lon);
  Serial.print(WP.alt);
  Serial.print(WP.id);
  Serial.println(WP.stg);
}
